#include "control.h"
#include "stdio.h"
#include "string.h"
#include "bsp_track.h"
#include "bsp_led.h"
#include "Delay.h"
#include "bsp_motor.h"
#include "motorencoder.h" 
#include "pid.h"
#include "bsp_mpu6050.h"
#include "imu_quat.h"
#include "filter.h"

#define Limit       99      // PWM波限幅，百分比制

extern PID_t pidMotorA;     // 速度环PID
extern PID_t pidMotorB;
extern PID_t pidAngle;      // 角度换PID

extern float currentA, currentB;    // 编码器读取到的电机当前速度，单位r/s
float targetA = 0, targetB = 0;     // A轮和B轮当前目标速度
extern float Yaw;                   // 陀螺仪偏转角返回值
float target = 0;                   // 小车目标角度
float Speed_Middle = 6;            // 直行速度 r/s

uint8_t led_flag = 0;    // 声光提示标志位
uint8_t motor_stop = 1;  // 电机停止标志位
float bias = 0;                     // 差速值

void Control_AB(void)   //模式一：AB直行
{
    /* 任一光电检测到黑色就停止 */
    if ((P1==BlackLine || P2==BlackLine || P3==BlackLine || P4==BlackLine
        || P5==BlackLine || P6==BlackLine || P7==BlackLine || P8==BlackLine) && motor_stop==0)
    {
        motor_stop = 1;
        // led_flag = 1;
        // Motor_Control(Motor_A, 1, 0);   // 停车
        // Motor_Control(Motor_B, 1, 0);
        LED_G_ON;
        Delay_ms(500);
        LED_G_OFF;
        // led_flag = 0;
    }

    /* 停止时声光提示 */
    // if (led_flag == 1) {
    // }
}

void Control_ABCDA(void)    //模式二：A->B->C->D
{
    static uint8_t targetPos = 'B'; // 记录目标位置
    static uint8_t quit_circle = 0; // 离开圆弧标志位，C点出弯标志位

    switch (targetPos)
    {
    case 'B': // A->B
        /* 任一光电检测到黑色就停止 */
        if ((P1==BlackLine || P2==BlackLine || P3==BlackLine || P4==BlackLine
            || P5==BlackLine || P6==BlackLine || P7==BlackLine || P8==BlackLine) && motor_stop==0)
        {
            motor_stop = 1;
            led_flag = 1;
        }
        /* 直走 */
        bias = 0;
        /* PID电机 */
        if (motor_stop == 0) {
            targetA = Speed_Middle + bias;
            targetB = Speed_Middle - bias;
            // 计算pid
            float pidOutA = PID_MotorCal(&pidMotorA, targetA, currentA);
            float pidOutB = PID_MotorCal(&pidMotorB, targetB, currentB);
            // 设置占空比
            Motor_Control(Motor_A, 1, pidOutA);
            Motor_Control(Motor_B, 1, pidOutB);
        } else if (motor_stop == 1) {
            Motor_Control(Motor_A, 1, 0);   // 停车
            Motor_Control(Motor_B, 1, 0);
            targetPos = 'C';                // 状态切换
        }
        break;
    case 'C': // B->C
        /* 所有光电检测不到黑色就停止，表示小车即将出弯，此时以航向角为准调一下车身 */
        if ((P1!=BlackLine && P2!=BlackLine && P3!=BlackLine && P4!=BlackLine
            && P5!=BlackLine && P6!=BlackLine && P7!=BlackLine && P8!=BlackLine) && motor_stop==0)
        {
            motor_stop = 1;
            led_flag = 1;
            quit_circle = 1;
        }
        /* 沿黑线走 */
        bias = Track_OutBias(0);
        /* 出弯判断 */
        // if (quit_circle = 1) {
        //     float tmpYaw = Yaw;
        //     if (tmpYaw<0) bias = 180 - myabs(tmpYaw); // 须将+180和-180转化为0+和0-
        //     else bias = tmpYaw - 180;
        //     quit_circle = 0;
        // }
        /* PID电机 */
        if (motor_stop == 0) {
            targetA = Speed_Middle + bias;
            targetB = Speed_Middle - bias;
            // 计算pid
            // 计算pid
            float pidOutA = PID_MotorCal(&pidMotorA, targetA, currentA);
            float pidOutB = PID_MotorCal(&pidMotorB, targetB, currentB);
            // 设置占空比
            Motor_Control(Motor_A, 1, pidOutA);
            Motor_Control(Motor_B, 1, pidOutB);
        } else if (motor_stop == 1) {
            Motor_Control(Motor_A, 1, 0);   // 停车
            Motor_Control(Motor_B, 1, 0);
            targetPos = 'D';                // 状态切换
        }
        break;
    case 'D': // C->D
        /* 任一光电检测到黑色就停止 */
        if ((P1==BlackLine || P2==BlackLine || P3==BlackLine || P4==BlackLine
            || P5==BlackLine || P6==BlackLine || P7==BlackLine || P8==BlackLine) && motor_stop==0)
        {
            motor_stop = 1;
            led_flag = 1;
        }
        /* 直走 */
        float tmpYaw = Yaw;
        if (tmpYaw<0) bias = 180 - myabs(tmpYaw);      // 须将+180和-180转化为0+和0-
        else bias = tmpYaw - 180;
        /* PID电机 */
        if (motor_stop == 0) {
            targetA = Speed_Middle + bias;
            targetB = Speed_Middle - bias;
            // 计算pid
            float pidOutA = PID_MotorCal(&pidMotorA, targetA, currentA);
            float pidOutB = PID_MotorCal(&pidMotorB, targetB, currentB);
            // 设置占空比
            Motor_Control(Motor_A, 1, pidOutA);
            Motor_Control(Motor_B, 1, pidOutB);
        } else if (motor_stop == 1) {
            Motor_Control(Motor_A, 1, 0);   // 停车
            Motor_Control(Motor_B, 1, 0);
            targetPos = 'A';                // 状态切换
        }
        break;
    case 'A': // D->A
        /* 所有光电检测不到黑色就停止 */
        if ((P1!=BlackLine && P2!=BlackLine && P3!=BlackLine && P4!=BlackLine
            && P5!=BlackLine && P6!=BlackLine && P7!=BlackLine && P8!=BlackLine) && motor_stop==0)
        {
            motor_stop = 1;
            led_flag = 1;
        }
        /* 沿黑线走 */
        bias = Track_OutBias(0);
        /* 驱动电机 */
        if (motor_stop == 0) {
            targetA = Speed_Middle + bias;
            targetB = Speed_Middle - bias;
            // 计算pid
            float pidOutA = PID_MotorCal(&pidMotorA, targetA, currentA);
            float pidOutB = PID_MotorCal(&pidMotorB, targetB, currentB);
            // 设置占空比
            Motor_Control(Motor_A, 1, pidOutA);
            Motor_Control(Motor_B, 1, pidOutB);
        } else if (motor_stop == 1) {
            Motor_Control(Motor_A, 1, 0);   // 停车
            Motor_Control(Motor_B, 1, 0);
        }
        break;
    default:
        break;
    }
    
    /* 停止时声光提示 */
    if (led_flag == 1) {
        LED_G_ON;
        Delay_ms(500);
        LED_G_OFF;
        led_flag = 0;
    }
}

void Control_ACBDA(void)                //模式三
{
    static uint8_t targetPos = 'C'; // 记录目标位置

    switch (targetPos)
    {
    case 'C': // A->C
        /* 任一光电检测到黑色就停止 */
        if ((P1==BlackLine || P2==BlackLine || P3==BlackLine || P4==BlackLine
            || P5==BlackLine || P6==BlackLine || P7==BlackLine || P8==BlackLine) && motor_stop==0)
        {
            motor_stop = 1;
            led_flag = 1;
        }
        /* 沿角度直行 */
        if (motor_stop == 0) {
            // 角度环
            target = Yaw  + 45; // 转角
            float pidAngleOut = PID_MotorCal(&pidAngle, target, Yaw);
            Motor_Control(Motor_A, 1, pidAngleOut);// A轮正转，B轮反转
            Motor_Control(Motor_B, 0, pidAngleOut);
            // 速度环
            bias = 0;
            targetA = Speed_Middle + bias;
            targetB = Speed_Middle - bias;
            float pidOutA = PID_MotorCal(&pidMotorA, targetA, currentA);
            float pidOutB = PID_MotorCal(&pidMotorB, targetB, currentB);
            Motor_Control(Motor_A, 1, pidOutA);
            Motor_Control(Motor_B, 1, pidOutB);
        } else if (motor_stop == 1) {
            Motor_Control(Motor_A, 1, 0);   // 停车
            Motor_Control(Motor_B, 1, 0);
            targetPos = 'B';                // 状态切换
        }
        break;
    case 'B': // C->B
        /* 所有光电检测不到黑色就停止，表示小车即将出弯给个出弯延时 */
        if ((P1!=BlackLine && P2!=BlackLine && P3!=BlackLine && P4!=BlackLine
            && P5!=BlackLine && P6!=BlackLine && P7!=BlackLine && P8!=BlackLine) && motor_stop==0)
        {
            motor_stop = 1;
            led_flag = 1;
        }
        /* 沿黑线走 */
        if (motor_stop == 0) {
            bias = Track_OutBias(0);
            targetA = Speed_Middle + bias;
            targetB = Speed_Middle - bias;
            // 计算pid
            float pidOutA = PID_MotorCal(&pidMotorA, targetA, currentA);
            float pidOutB = PID_MotorCal(&pidMotorB, targetB, currentB);
            // 设置占空比
            Motor_Control(Motor_A, 1, pidOutA);
            Motor_Control(Motor_B, 1, pidOutB);
        } else if (motor_stop == 1) {
            Motor_Control(Motor_A, 1, 0);   // 停车
            Motor_Control(Motor_B, 1, 0);
            targetPos = 'D';                // 状态切换
        }
        break;
    case 'D': // B->D
        /* 任一光电检测到黑色就停止 */
        if ((P1==BlackLine || P2==BlackLine || P3==BlackLine || P4==BlackLine
            || P5==BlackLine || P6==BlackLine || P7==BlackLine || P8==BlackLine) && motor_stop==0)
        {
            motor_stop = 1;
            led_flag = 1;
        }
        if (motor_stop == 0) {
            /* 沿偏航角反向直走 */
            float tmpYaw = Yaw;
            if (tmpYaw<0) target = 180 - myabs(tmpYaw);      // 须将+180和-180转化为0+和0-
            else target = tmpYaw - 180;
            // 角度环
            float pidAngleOut = PID_MotorCal(&pidAngle, target, Yaw);
            Motor_Control(Motor_A, 0, pidAngleOut);// A轮反转，B轮正转
            Motor_Control(Motor_B, 1, pidAngleOut);
            // 速度环
            bias = 0;
            targetA = Speed_Middle + bias;
            targetB = Speed_Middle - bias;
            float pidOutA = PID_MotorCal(&pidMotorA, targetA, currentA);
            float pidOutB = PID_MotorCal(&pidMotorB, targetB, currentB);
            Motor_Control(Motor_A, 1, pidOutA);
            Motor_Control(Motor_B, 1, pidOutB);
        } else if (motor_stop == 1) {
            Motor_Control(Motor_A, 1, 0);   // 停车
            Motor_Control(Motor_B, 1, 0);
            targetPos = 'A';                // 状态切换
        }
        break;
    case 'A': // D->A
        /* 所有光电检测不到黑色就停止 */
        if ((P1!=BlackLine && P2!=BlackLine && P3!=BlackLine && P4!=BlackLine
            && P5!=BlackLine && P6!=BlackLine && P7!=BlackLine && P8!=BlackLine) && motor_stop==0)
        {
            motor_stop = 1;
            led_flag = 1;
        }
        /* 沿黑线走 */
        if (motor_stop == 0) {
            bias = Track_OutBias(0);
            targetA = Speed_Middle + bias;
            targetB = Speed_Middle - bias;
            // 计算pid
            float pidOutA = PID_MotorCal(&pidMotorA, targetA, currentA);
            float pidOutB = PID_MotorCal(&pidMotorB, targetB, currentB);
            // 设置占空比
            Motor_Control(Motor_A, 1, pidOutA);
            Motor_Control(Motor_B, 1, pidOutB);
        } else if (motor_stop == 1) {
            Motor_Control(Motor_A, 1, 0);   // 停车
            Motor_Control(Motor_B, 1, 0);
        }
        break;
    default:
        break;
    }
    
    /* 停止时声光提示 */
    if (led_flag == 1) {
        LED_G_ON;
        Delay_ms(500);
        LED_G_OFF;
        led_flag = 0;
    }
}

void Control_ACBDAx4(void)
{
    static uint8_t targetPos = 'C'; // 记录目标位置
    uint8_t circle = 0;             // 记录圈数

    switch (targetPos)
    {
    case 'C': // A->C
        /* 任一光电检测到黑色就停止 */
        if ((P1==BlackLine || P2==BlackLine || P3==BlackLine || P4==BlackLine
            || P5==BlackLine || P6==BlackLine || P7==BlackLine || P8==BlackLine) && motor_stop==0)
        {
            motor_stop = 1;
            led_flag = 1;
        }
        /* 沿角度直行 */
        if (motor_stop == 0) {
            // 角度环
            target = Yaw  + 45; // 转角
            float pidAngleOut = PID_MotorCal(&pidAngle, target, Yaw);
            Motor_Control(Motor_A, 1, pidAngleOut);// A轮正转，B轮反转
            Motor_Control(Motor_B, 0, pidAngleOut);
            // 速度环
            bias = 0;
            targetA = Speed_Middle + bias;
            targetB = Speed_Middle - bias;
            float pidOutA = PID_MotorCal(&pidMotorA, targetA, currentA);
            float pidOutB = PID_MotorCal(&pidMotorB, targetB, currentB);
            Motor_Control(Motor_A, 1, pidOutA);
            Motor_Control(Motor_B, 1, pidOutB);
        } else if (motor_stop == 1) {
            Motor_Control(Motor_A, 1, 0);   // 停车
            Motor_Control(Motor_B, 1, 0);
            targetPos = 'B';                // 状态切换
        }
        break;
    case 'B': // C->B
        /* 所有光电检测不到黑色就停止，表示小车即将出弯给个出弯延时 */
        if ((P1!=BlackLine && P2!=BlackLine && P3!=BlackLine && P4!=BlackLine
            && P5!=BlackLine && P6!=BlackLine && P7!=BlackLine && P8!=BlackLine) && motor_stop==0)
        {
            motor_stop = 1;
            led_flag = 1;
        }
        /* 沿黑线走 */
        if (motor_stop == 0) {
            bias = Track_OutBias(0);
            targetA = Speed_Middle + bias;
            targetB = Speed_Middle - bias;
            // 计算pid
            float pidOutA = PID_MotorCal(&pidMotorA, targetA, currentA);
            float pidOutB = PID_MotorCal(&pidMotorB, targetB, currentB);
            // 设置占空比
            Motor_Control(Motor_A, 1, pidOutA);
            Motor_Control(Motor_B, 1, pidOutB);
        } else if (motor_stop == 1) {
            Motor_Control(Motor_A, 1, 0);   // 停车
            Motor_Control(Motor_B, 1, 0);
            targetPos = 'D';                // 状态切换
        }
        break;
    case 'D': // B->D
        /* 任一光电检测到黑色就停止 */
        if ((P1==BlackLine || P2==BlackLine || P3==BlackLine || P4==BlackLine
            || P5==BlackLine || P6==BlackLine || P7==BlackLine || P8==BlackLine) && motor_stop==0)
        {
            motor_stop = 1;
            led_flag = 1;
        }
        /* 沿偏航角反向直走 */
        if (motor_stop == 0) {
            float tmpYaw = Yaw;
            if (tmpYaw<0) bias = 180 - myabs(tmpYaw);      // 须将+180和-180转化为0+和0-
            else bias = tmpYaw - 180;
            // 角度环
            float pidAngleOut = PID_MotorCal(&pidAngle, target, Yaw);
            Motor_Control(Motor_A, 0, pidAngleOut);// A轮反转，B轮正转
            Motor_Control(Motor_B, 1, pidAngleOut);
            // 速度环
            bias = 0;
            targetA = Speed_Middle + bias;
            targetB = Speed_Middle - bias;
            float pidOutA = PID_MotorCal(&pidMotorA, targetA, currentA);
            float pidOutB = PID_MotorCal(&pidMotorB, targetB, currentB);
            Motor_Control(Motor_A, 1, pidOutA);
            Motor_Control(Motor_B, 1, pidOutB);
        } else if (motor_stop == 1) {
            Motor_Control(Motor_A, 1, 0);   // 停车
            Motor_Control(Motor_B, 1, 0);
            targetPos = 'A';                // 状态切换
        }
        break;
    case 'A': // D->A
        /* 所有光电检测不到黑色就停止 */
        if ((P1!=BlackLine && P2!=BlackLine && P3!=BlackLine && P4!=BlackLine
            && P5!=BlackLine && P6!=BlackLine && P7!=BlackLine && P8!=BlackLine) && motor_stop==0)
        {
            motor_stop = 1;
            led_flag = 1;
        }
        /* 沿黑线走 */
        if (motor_stop == 0) {
            bias = Track_OutBias(0);
            targetA = Speed_Middle + bias;
            targetB = Speed_Middle - bias;
            // 计算pid
            float pidOutA = PID_MotorCal(&pidMotorA, targetA, currentA);
            float pidOutB = PID_MotorCal(&pidMotorB, targetB, currentB);
            // 设置占空比
            Motor_Control(Motor_A, 1, pidOutA);
            Motor_Control(Motor_B, 1, pidOutB);
        } else if (motor_stop == 1) {
            Motor_Control(Motor_A, 1, 0);   // 停车
            Motor_Control(Motor_B, 1, 0);
            targetPos = 'C';                // 状态切换
            circle++;
            if (circle == 4) {
                targetPos = '0';// 四圈跑完
            }
        }
        break;
    default:
        break;
    }
    
    /* 停止时声光提示 */
    if (led_flag == 1) {
        LED_G_ON;
        Delay_ms(500);
        LED_G_OFF;
        led_flag = 0;
    }
}

/**
 * @brief 输出差速，左侧检测到黑线，给左轮减速
 * 
 * @param mode  0，使用全部灰度
 *              1，使用左半部分（P1~P4）灰度
 *              2，使用右半部分（P5~P8）灰度
 * @return int 输出差速，左轮速度为middle+x，右轮速度为middle-x
 */
int Track_OutBias(uint8_t mode)
{
    switch (mode)
    {
    case 0:
        /* 使用全部灰度 */
        if(P1==BlackLine)  return -6;
        else if(P2==BlackLine)  return -4;
        else if(P3==BlackLine)  return -2;
        else if(P4==BlackLine)  return -1;
        else if(P5==BlackLine)  return 1;
        else if(P6==BlackLine)  return 2;
        else if(P7==BlackLine)  return 4;
        else if(P8==BlackLine)  return 6;
        break;
    case 1:
        /* 使用左半部分灰度 */
        if(P1==BlackLine)  return -6;
        else if(P2==BlackLine)  return -4;
        else if(P3==BlackLine)  return -2;
        else if(P4==BlackLine)  return -1;
        break;
    case 2:
        /* 使用右半部分灰度 */
        if(P5==BlackLine)  return 1;
        else if(P6==BlackLine)  return 2;
        else if(P7==BlackLine)  return 4;
        else if(P8==BlackLine)  return 6;
        break;
    default:
        break;
    }
    return 0;
}

//pwm限幅
float PWM_Limit(float IN, float max, float min)
{
    float OUT = IN;
    if(OUT > max) OUT = max;
    if(OUT < min) OUT = min;
    return OUT;
}

//自定义绝对值函数
int myabs(int a)
{
    int temp;
    if(a < 0)  temp = -a;
    else temp = a;
    return temp;
}

/* 变量 */
extern MPU6050_t* MPU6050;
extern EulerAngle_t angle;

/* 标志位 */
extern uint8_t carMode;        // 小车运动模式
extern uint8_t carRun;         // 小车启动标志位
void TIM_CallBack()
{
    // 3ms进入一次中断
    static uint16_t Cnt_900ms = 0;  // 900ms 计数符
    static uint16_t Cnt_3ms = 0;    // 3ms   计数符
    static uint16_t Cnt_6ms = 0;    // 6ms   计数符
    static uint16_t Cnt_15ms = 0;   // 15ms  计数符

    /* 小车开启才会执行 */
    if (carRun)
    {
        Cnt_900ms++;
        Cnt_3ms++;
        Cnt_6ms++;
        Cnt_15ms++;
        
        // (执行频次 3ms) 传感器数据更新 + PID控制 + 电机控制
        if (Cnt_3ms >= 1) {
            Cnt_3ms = 0;

            IMU_GetData(MPU6050);   // 读取 MPU6050数据（已滤波）
        }

        // (执行频次 15ms) 编码器读取
        if (Cnt_15ms >= 5) {
            Cnt_15ms = 0;
            // 15ms读取一次编码器(即1000/15HZ)，电机减速比为20，霍尔编码器精度13，AB双相组合得到4倍频，
            // 则转1圈编码器读数为20*13*4=1040，电机转速=Encoder*1000/15/1040r/s
            currentA = Encoder_Read(Motor_A);
            currentB = Encoder_Read(Motor_B);
            // printf("%d,%d,", (int16_t)currentA, (int16_t)currentB);
            currentA = currentA * 1000/15.0 / 1040.0;   // 单位转换
            currentB = currentB * 1000/15.0 / 1040.0;
            printf("%f,%f,", currentA, currentB);
            currentA = (currentA > 0)?currentA:-currentA;// 符号转换
            currentB = (currentB > 0)?currentB:-currentB;
            /* 电机前进或停止 */
            if (motor_stop == 0) {
                targetA = Speed_Middle + bias;  // 差速
                targetB = Speed_Middle - bias;
                // 计算pid
                float pidOutA = PID_MotorCal(&pidMotorA, targetA, currentA);
                float pidOutB = PID_MotorCal(&pidMotorB, targetB, currentB);
                // 设置占空比
                Motor_Control(Motor_A, 1, pidOutA);
                Motor_Control(Motor_B, 1, pidOutB);
            }
            else {
                Motor_Control(Motor_A, 1, 0);
                Motor_Control(Motor_B, 1, 0);
            }

            // PID_MotorCal(&pidMotorA, 4, (currentA > 0)?currentA:-currentA);
            // PID_MotorCal(&pidMotorB, 4, (currentB > 0)?currentB:-currentB);
            // printf("%f,%f\n", pidMotorA.out, pidMotorB.out);
            // Motor_Control(Motor_A, 1, pidMotorA.out);
            // Motor_Control(Motor_B, 1, pidMotorB.out);
        }
        
        // (执行频次 6ms) 欧拉角更新
        if (Cnt_6ms >= 2) {
            Cnt_6ms = 0;
            
            IMU_Update(MPU6050, &angle, 0.006f);
            Yaw = angle.yaw;
            // printf("%.2f,%.2f,%.2f\n", angle.roll, angle.pitch, angle.yaw);
            // printf("%.2f,", angle.yaw);
        }

        // (执行频次 900ms) 连接状态检查 + 失连动作
        if (Cnt_900ms >= 300) {
            Cnt_900ms = 0;
            // printf("roll: %.2f || pitch: %.2f || yaw: %.2f\n", angle.roll, angle.pitch, angle.yaw);
            // printf("ax: %.2f || ay: %.2f || az: %.2f\n", MPU6050->ax, MPU6050->ay, MPU6050->az);
            // printf("gx: %.2f || gy: %.2f || gz: %.2f\n", MPU6050->gx, MPU6050->gy, MPU6050->gz);
            // printf("temp: %2.f\n", MPU6050->temp);
        }
        
    }
}
